#ifndef ARMOR_DETECT_H
#define ARMOR_DETECT_H

#include<opencv2/opencv.hpp>
#include<SVC_v2/sizeclassifiersvc.h>
#include"distance_filter.h"
#include"predictmove.h"
#include<iostream>

using namespace std;
using namespace cv;

//debug/
//#define SHOW_IMG
#define SHOW_SRC
//#define LIGHT_INFO
//#define DRAW_LIGHT
//#define DRAW_LIGHT_MATCH
//#define ARMOR_INFO
//#define MATCH_DEEPER
//#define DRAW_MATCH_DEEPER
//#define TAKE_PHOTO
//#define RECORD



#define VIDEO_WIDTH 1280
#define VIDEO_HEIGHT 1024
#define BLUE_THRESH 80
#define BLUE_THRESH_F 85
#define RED_THRESH 40
#define RED_THRESH_F 45
#define SENTRY_BLUE_THRESH 220
#define SENTRY_RED_THRESH 45
#define GRAY_THRESH 100

#define MIN_LIGHT_AREA 20
#define MIN_LIGHT_ASPECT_RATIO 1.5
#define MAX_LIGHT_ASPECT_RATIO 10
#define RIGHT_TILT_ANGLE 30
#define LEFT_TILT_ANGLE 150

#define MIN_ARMOR_ASPECT_RATIO 1.2
#define MAX_ARMOR_ASPECT_RATIO 5.2
#define MIN_HEIGHT_RATIO 0.5
#define MAX_HEIGHT_RATIO 2
#define MAX_LIGHTS_PARALLELISM 0.8
#define MAX_SMALL_ARMOR_TILT_ANGLE 0.5
#define MAX_BIG_ARMOR_TILT_ANGLE 0.15
#define MAX_ANGLE_ERROR1 12
#define MAX_ANGLE_ERROR2 8
#define MAX_Y_DIFF_RATIO 1

#define ANGLE_NEAR_ZERO 88

#define ROI_DROP_FRAME_BOUND 25
#define ROI_WIDTH_SCALE 0.65;
#define ROI_HEIGHT_SCALE 1;
#define DROP_ROI_HEIGHT_SCALE 0.3;
#define DROP_ROI_WIDTH_SCALE 0.3;

#define small_armor_weight 67.5
#define small_armor_height 28
#define big_armor_weight 115
#define big_armor_height 28

struct Data{
    int mode = 0 ;
    int my_color = 1;  //1代表
    int shoot_speed = 15;
    double gyro_pitch = 0;
    double gyro_yaw = 0;

    int anti_flag = 0;
    uint32_t time_stamp;
    void reset();
};

struct Light{
    double area;
    Light()
    {
        area = 0;
        titl_direction=0;
        light_rect = RotatedRect();
    }

    int titl_direction;
    RotatedRect light_rect;
    Point top;
    Point bottom;
    Point center;


};

struct Armor_params
{
    double big_max_armor_aspect_ratio;
    double big_min_armor_aspect_ratio;
    double small_max_armor_aspect_ratio;
    double small_min_armor_aspect_ratio;
    double min_height_ratio;
    double min_armor_rect_angle;
    double angle_differ1;
    double angle_differ2;
    double max_small_armor_tilt_angle;
    double max_big_armor_tilt_angle;
    double max_y_diff_ratio1;
    double max_y_diff_ratio2;
    Armor_params(int mode)
    {
        if(mode == 1 or mode == 4 )
        {
            big_max_armor_aspect_ratio=4.8;
            big_min_armor_aspect_ratio=3.6;
            small_max_armor_aspect_ratio=4;
            small_min_armor_aspect_ratio=1.2;
            min_height_ratio = 0.5;

            angle_differ1=15;
            angle_differ2=4;


            max_small_armor_tilt_angle = 0.5;
            max_big_armor_tilt_angle = 0.35;
            min_armor_rect_angle =30;

            max_y_diff_ratio1 = 1.3;
            max_y_diff_ratio2 = 0.6;
        }
    }

};

struct Armor{
    double pitch = 0;
    double yaw = 0;
    double distance = 0;
    double type = 2; //0:small  1: big  2:empty
    int ID = 0;
    int area = 0;
    Point left_top;
    Point right_top;
    Point left_bottom;
    Point right_bottom;
    RotatedRect armor_rect;
    Rect bounding_rect;

    bool empty = 1;

    void armor_reset();

};

struct ArmorDetector{

    Mat src;                //源图
    Mat img;                //roi
    Mat binary_img;         //二值图

    double now_time_stamp;
    double pre_time_stamp;

    double center_x=0;
    double center_y=0;


    Point roi_tl;
    int drop_frame_cnt = 0;
    bool roi_init =0;
    Rect roi_rect;

    vector<Light>lights;            //每一帧都要清空
    Light right_light;
    Light left_light;
    vector<Point> last_point;
    vector<Light>first_result;      //每一帧都要清空
    vector<int>first_result_index;  //每一帧都要清空
    vector<Light>result;            //每一帧都要清空

    Armor target_armor;
    Armor pre_target;
    void reset();
    void ctrl_reset();

    double pnp_dis;



    void find_light();
    Light adjust_light(Light &light);
    void adjust_rect(RotatedRect &rect);
    void match_lights(int mode);
    void match_lights_deeper();
    void get_armor_info(Mat cam, Mat dis);
    void choose_target(int my_color);
    Mat get_num_img(Mat pad_img);

    void set_img(bool use_roi);
    void pretreat_img(int my_color, int mode);
    void info_src(Data rdata);
    void get_tvec(Armor &armor, Mat cam, Mat dis);
    void armor_ctrl(bool use_roi, Mat cam, Mat dis, Data rdata);
    void set_offset(double target_dis, double &target_pit, double &target_yaw, int shoot_speed, int mode, int my_color);
    SizeClassifierSVC  clf;

};

struct ArmorTracker{
    vector<Armor> armors;   //不清空
    int find_cnts;
    int loss_cnts;

    bool tracking_flag;

    void get_armor_info();
    Mat get_tvec(Mat cam, Mat dis);
    int get_armor_ID();
    void choose_target();

};



Point get_max_y_point(vector<Point> points);
Point get_min_y_point(vector<Point> points);
Point get_max_x_point(vector<Point> points);
Point get_min_x_point(vector<Point> points);
#endif // ARMOR_DETECT_H
